卡尔曼滤波是什么

你可以在任何具有不确定性信息的动态系统上应用卡尔曼滤波，它可以告诉你系统下一步的演进方向。

卡尔曼滤波尤其适用于连续变化的系统，其优点包括: 运算速度快以及占用存储空间少(除上一时刻的状态外不需要存储其它历史信息)。因此，对要求实时性强、内存占用少的嵌入式应用场景非常合适。

卡尔曼滤波能做什么

简单的例子

卡尔曼滤波模型

为简单起见，假定系统状态仅包含位置和速度两个变量，即

x=[pv]\mathbf{x}=\left[ \begin{array}{c} p \\ v \end{array} \right]

我们不知道真实的位置和速度是多少，但在位置和速度取值区间里，有一些位置和速度的组合比其它组合可能性更大。位置和速度的分布如下图:

图片

卡尔曼滤波假定变量(这里包括位置和速度)均为随机变量且服从高斯分布。每个变量均有对应的均值μ\mathbf{\mu}，即随机变量的中心(也就是可能性最大的状态)，以及方差σ2\mathbf{\sigma}^2，代表不确定性。

图片

在上面图片示意中，位置和速度是不相关的，即一个变量的状态不影响另一变量的取值。但我们对变量相关的情况可能更感兴趣，下图描述了速度取值影响位置状态可能性的情况: 图片

举例，当我们希望通过旧位置估计一个新位置状态时对应的就是这种情况。如果速度较快，我们可能走得更远，所以位置取值大的可能性更高。反之，如果移动较慢，则不会走得很远。

类似的关系至关重要，因为其能够提供更多信息: 观测到其中某一个变量将告诉我们其它变量可能的情况。这也是卡尔曼滤波的目的，即从不确定的观测中获取尽可能多的信息。

这类关系可以通过协方差矩阵来描述。简而言之，矩阵中的每个元素Σij\Sigma_{ij}表示第ii个状态变量与第jj个状态变量的相关程度。由于ii与jj和jj与ii的相关程度一致，因此协方差矩阵是对称阵。

图片

矩阵表示

我们利用高斯分布来表示状态信息，则对kk时刻需要两个量来描述: 最优状态估计x^k\hat{\mathbf{x}}_k(对应均值)以及对应的协方差矩阵Pk\mathbf{P}_k.

x^k=[pv]Pk=[ΣppΣpvΣvpΣvv]\begin{equation} \begin{align*} \hat{\mathbf{x}}_k &=\left[ \begin{array}{c} p \\ v \end{array} \right] \\ \mathbf{P}_k &=\left[ \begin{array}{cc} \Sigma_{pp} & \Sigma_{pv} \\ \Sigma_{vp} & \Sigma_{vv} \end{array} \right] \end{align*} \end{equation}

接下来，我们需要合适的方式来看待当前状态(k−1k-1时刻)以及预测下一时刻状态(kk时刻)。注意，我们不知道真实的状态，但这并不影响预测函数。我们可以对k−1k-1时刻所有可能的状态进行预测推演，这样我们将得到一个新的分布(如下图): 图片

我们可以将预测这一步通过矩阵Fk\mathbf{F}_k来描述: 它将当前状态估计的每个点移动到一个新的位置。如果当前状态是正确的，则它代表了系统演进后可能处在状态。 图片

我们这里使用一个简单的运动学模型:

pk=pk−1+Δtvk−1vk=vk−1\begin{align*} {\color{Magenta}{p_k}} &= {\color{blue}p_{k-1}} + \Delta t &{\color{blue}v_{k-1}} \\ {\color{Magenta}{v_k}} &= &{\color{blue}v_{k-1}} \end{align*}

用矩阵形式可以表示为:

x^k=[1Δt01]x^k−1=Fkx^k−1\begin{align} {\color{Magenta} \hat{\mathbf{x}}_k} &= \left[ \begin{array}{cc} 1 & \Delta t \\ 0 & 1 \end{array} \right] {\color{blue} \hat{\mathbf{x}}_{k-1}} \\ &=\mathbf{F}_k {\color{Blue} \hat{\mathbf{x}}_{k-1}} \end{align}

我们现在有一个预测矩阵来获得下一时刻状态, 但我们仍然不知道怎么更新协方差矩阵。

这里我们需要另外的公式。如果我们将一个分布中的所有点都乘以矩阵A\mathbf{A}, 那么协方差矩阵会怎么变化呢？

事实上，这很简单。这里直接给出结论:

Cov(x)=ΣCov(Ax)=AΣAT\begin{equation} \begin{align*} \text{Cov}(x) &= \Sigma \\ \text{Cov}({\color{Maroon} \mathbf{A}}x) &= {\color{Maroon} \mathbf{A}} \Sigma {\color{Maroon} \mathbf{A}}^T \end{align*} \end{equation}

因此，结合公式(4)和公式(3):

x^k=Fkx^k−1P^k=FkP^k−1FkT\begin{equation} \begin{align*} {\color{Magenta} \hat{\mathbf{x}}_k} &= \mathbf{F}_k {\color{Blue} \hat{\mathbf{x}}_{k-1}} \\ {\color{Magenta} \hat{\mathbf{P}}_k} &= \mathbf{F}_k {\color{Blue} \hat{\mathbf{P}}_{k-1}} \mathbf{F}_k^T \end{align*} \end{equation}

外部影响

上述模型并没有考虑所有的因素。现实环境中，状态受除状态外的其它因素影响 —— 外部环境可能会影响系统本身。

例如，如果状态用于对列车运动建模，列车操作员可能加速杆，让列车突然加速。类似地，在机器人的例子中，导航软件可能发出转向或停止的命令。如果我们知道外部这些额外的信息，我们可以将其统一通过一个向量uk\mathbf{u}_k来表示，并将其加入到我们的预测中以对预测结果进行矫正。

假定我们可以通过加速杆或控制指令知道预期的加速度aa，通过基本运动学我们有：

pk=pk−1+Δtvk−1+12aΔt2vk=vk−1+aΔt\begin{align*} {\color{Magenta}p_k} &= {\color{Blue}p_{k-1}} + \Delta t {\color{Blue} v_{k-1}} + \frac{1}{2} {\color{Orange}a} \Delta t^2 \\ {\color{Magenta} v_k} &= {\color{Blue}v_{k-1}} + {\color{Orange} a}\Delta t \end{align*}

其矩阵形式:

x^k=Fkx^k−1+[Δt22Δt]a=Fkx^k−1+Bkuk\begin{equation} \begin{align*} {\color{Magenta}\hat{\mathbf{x}}_k} &= \mathbf{F}_k{\color{Blue}\hat{\mathbf{x}}_{k-1}} + \left[ \begin{array}{c} \frac{\Delta t^2}{2} \\ \Delta t \end{array} \right]{\color{Orange} a} \\ &= \mathbf{F}_k{\color{Blue}\hat{\mathbf{x}}_{k-1}} + \mathbf{B}_k{\color{Orange} \mathbf{u}_k} \end{align*} \end{equation}

Bk\mathbf{B}_k称为控制矩阵，uk{\color{Orange} \mathbf{u}_k}称为控制向量。对没有外部影响的简单系统，可以忽略这一项。

更近一步，如果模型不能100%100\%精准地表示真实系统的运行，预测会受到怎样的影响？

外部不确定性

如果状态基于其自身性质演进，上面的模型完全没问题。若状态受外部作用力演进，如果相关的外部作用力已知，上述模型也没问题。

但是，对我们不知道的外部作用力呢？例如，如果我们对一架无人机进行追踪，它可能受风力的影响。又如对轮型驱动的机器人跟踪时，轮子可能出现打滑或者卡住导致其速度减缓。我们无法对这些因素进行建模跟踪，因此当其发生时，预测结果会出错，因为我们没有考虑到这些额外的作用力。

我们可以将“世界”(即包括所有我们无法跟踪的因素)的不确定性建模为每次预测后新增部分不确定性，如下图

图片

原先估计中的每个状态可能移动到一个范围内的状态。因为我们非常喜欢分布，让我们假定每个点x^k−1{\color{Blue} \hat{\mathbf{x}}_{k-1}}移动到协方差为Qk\color{Cyan}\mathbf{Q}_k的高斯斑内，换句话说，即我们将未能跟踪的影响通过协方差为Qk\color{Cyan}\mathbf{Q}_k的噪声来建模。如下图

图片

这将产生一个新的高斯斑，它们具有相同的均值但不同的协方差，如下图所示: 图片

我们通过简单加Qk\color{Cyan}\mathbf{Q}_k来得到期望的协方差矩阵。因此，完整的预测可以表示如下:

x^k=Fkx^k−1+BkukPk=FkPk−1FkT+Qk\begin{equation} \begin{align*} {\color{Magenta} \hat{\mathbf{x}}_k} &= \mathbf{F}_k{\color{Blue} \hat{\mathbf{x}}_{k-1}} + \mathbf{B}_k{\color{Orange} \mathbf{u}_k} \\ {\color{Magenta} {\mathbf{P}}_k} &= \mathbf{F}_k {\color{Blue} {\mathbf{P}}_{k-1}}\mathbf{F}_k^T + {\color{Cyan}\mathbf{Q}_k} \end{align*} \end{equation}

换而言之，

ok, 我们通过系统自身演进以及外部作用得到系统状态的模糊估计，并通过x^k{\color{Magenta} \hat{\mathbf{x}}_k}和Pk{\color{Magenta} {\mathbf{P}}_k}表示。如果我们能通过传感器获得一些相关数据又会怎样呢？

通过观测优化估计

我们可能有若干传感器为我们提供系统状态信息。也许一个读取位置，另一个读取速度，但当前先不管它们测量啥。每个传感器间接告诉我们一些关于状态的信息 —— 换句话说，传感器基于系统状态产生系列数据。

图片

注意传感器读数的单位和尺度可能与系统状态的单位和尺度不是一致的。与之前类似，我们可以通过变换矩阵Hk\mathbf{H}_k对传感器建模:

图片

可以推导出观测数据分布与状态的关系:

μexpected=Hkx^kΣexpected=HkPkHkT\begin{equation} \begin{align*} \mathbf{\mu}_{\text{expected}} &= \mathbf{H}_k{\color{Magenta} \hat{\mathbf{x}}_k} \\ \mathbf{\Sigma}_{\text{expected}} & = \mathbf{H}_k{\color{Mageta} \mathbf{P}_k}\mathbf{H}_k^T \end{align*} \end{equation}

卡尔曼滤波的一大优点是处理传感器噪声。换而言之，我们的传感器在某种程度上都是不可靠的，先前估计的每个状态都可能导致一个范围的传感器读数。

图片

对观测到的每个读数，我们可以推测系统可能处在某个状态。由于不确定性存在，我们无法根据观测读数得到精准的状态，但某些状态产生观测读数的可能性要比其它状态更高:

图片

我们将该不确定性对应的协方差记为Rk{\color{cyan} \mathbf{R}_k}。分布的均值等于我们的观测读数，定义为zk{\color{green}\mathbf{z}_k}。

因此，我们现在有两个高斯分布，一个以变换后预测的均值为中心，另一个以真实传感器读数为中心。

我们需要对基于预测状态(粉红色)推测的读数猜测与实际传感器读数(绿色)进行协调。

因此我们最可能的状态是？对任意可能的读数(z1,z2)(z_1, z_2)，我们有两个关联的概率: (1)传感器读数zk\mathbf{z}_k属于(z1,z2)(z_1, z_2)观测的概率；(2)上一状态预测认为(z1,z2)(z_1, z_2)应该是观测到读数的概率。

如果我们有两个概率并且希望知道两个均为真的概率，我们只需要将它们相乘。因此，我们将两个高斯分布相乘可得:

相乘得到的是重叠区域，该区域内变量属于这两个分布的可能性均较高。因而，能够得到比之前更准确的估计。相乘后得到分布的均值对两个分布来说都是最有可能的，因此基于我们有的信息能获得的最佳估计。

当将两个具有独立均值和方差的高斯分布相乘时，得到一个新高斯分布，具有其自身的均值和方差。可能你已看到接下来讲什么: 从旧的高斯分布得到新高斯分布参数的公式是什么。

组合高斯

让我们来看下公式。为了便于理解，先来看一维的情况。均值为μ\mu方差为σ2\sigma^2的一维高斯分布可表示为:

N(x,μ,σ)=1σ2πexp⁡{−(x−μ)2/(2σ2)}\begin{equation} \mathcal{N}(x,\mu,\sigma)=\frac{1}{\sigma\sqrt{2\pi}}\exp\{-(x-\mu)^2/(2\sigma^2)\} \end{equation}

我们想知道当将两个高斯分布相乘时会发生什么。下面蓝色曲线是两个高斯分布的相交部分的表示:

N(x,μ0,σ0)⋅N(x,μ1,σ1)=N(x,μ′,σ′)\begin{equation} \mathcal{N}(x,{\color{magenta}{\mu_0}},{\color{magenta}{\sigma_0}}) \cdot \mathcal{N}(x,{\color{green}{\mu_1}},{\color{green}{\sigma_1}}) =\mathcal{N}(x,{\color{blue}{\mu'}},{\color{blue}{\sigma'}}) \end{equation}

将公式(9)带入到公式(10)后做些运算可得:

μ′=μ0+σ02(μ1−μ0)σ02+σ12σ′2=σ02−σ04σ02+σ12\begin{align*} {\color{blue}{\mu'}} &= \mu_0 + \frac{\sigma^2_0(\mu_1-\mu_0)}{\sigma^2_0 + \sigma^2_1}\\ {\color{blue}{\sigma}'^{2}} &= \sigma^2_0 - \frac{\sigma^4_0}{\sigma^2_0 + \sigma^2_1} \end{align*}

我们可以把其中的一小部分提出来并定义为k\color{purple}{\mathbf{k}}

k=σ02σ02+σ12μ′=μ0+k(μ1−μ0)σ′2=σ02−kσ02\begin{align*} {\color{purple}\mathbf{k}} &= \frac{\sigma^2_0}{\sigma^2_0 + \sigma^2_1} \\ {\color{blue}\mu'} &= \mu_0 + {\color{purple}\mathbf{k}}(\mu_1-\mu_0) \\ {\color{blue}\sigma'^2} &= \sigma^2_0 - {\color{purple}\mathbf{k}}\sigma^2_0 \end{align*}

注意到你可以基于之前的估计然后加上一些增量来获得新的估计，并且可以看到公式是多么简单！

那对于多维高斯呢？它们的最终形式是一致的。假定高斯分布的均值为μ\boldsymbol{\mu}，协方差为Σ\boldsymbol{\Sigma}，则

K=Σ0(Σ0+Σ1)−1μ′=μ0+K(μ1−μ0)Σ′=Σ0−KΣ0\begin{align*} {\color{purple} \mathbf{K}} &= \boldsymbol{\Sigma}_0(\boldsymbol{\Sigma}_0 + \boldsymbol{\Sigma}_1)^{-1} \\ {\color{blue} \boldsymbol{\mu}'} &= \boldsymbol{\mu}_0 + {\color{purple}\mathbf{K}}(\boldsymbol{\mu}_1 - \boldsymbol{\mu}_0) \\ {\color{blue}\boldsymbol{\Sigma}'} &= \boldsymbol{\Sigma}_0 - {\color{purple} \mathbf{K}}\boldsymbol{\Sigma}_0 \end{align*}

其中，K\color{purple} \mathbf{K}矩阵为卡尔曼增益，我们过会会使用到。

整合一起

我们有两个分布:

我们按上述方法融合两个分布:

Hkx^k′=Hkx^k+K(zk−Hkx^k)HkPk′HkT=HkPkHkT−KHkPkHkT\begin{align*} \mathbf{H}_k{\color{blue}\hat{\mathbf x}_k'} &= {\color{magenta}\mathbf{H}_k\hat{\mathbf{x}}_k} + {\color{purple}\mathbf{K}}({\color{green}\mathbf{z}_k}-{\color{magenta}\mathbf{H}_k\hat{\mathbf{x}}_k}) \\ \mathbf{H}_k{\color{blue}\mathbf{P}_k'}\mathbf{H}^T_k &= {\color{orangered}\mathbf{H}_k\mathbf{P}_k\mathbf{H}^T_k} - {\color{purple}\mathbf{K}}{\color{orangered}\mathbf{H}_k\mathbf{P}_k\mathbf{H}^T_k} \end{align*}

其中，卡尔曼增益为:

K=HkPkHkT(HkPkHkT+Rk)−1{\color{purple}\mathbf{K}} = {\color{orangered}\mathbf{H}_k\mathbf{P}_k\mathbf{H}^T_k}({\color{orangered}\mathbf{H}_k\mathbf{P}_k\mathbf{H}^T_k} + {\color{cyan}\mathbf{R}_k})^{-1}

消去上述计算公式前的Hk\mathbf{H}_k(注，包括一个隐藏在K\color{purple}\mathbf{K}前的)以及协方差后的HkT\mathbf{H}^T_k，可得

x^k′=x^k+K′(zk−Hkx^k)Pk′=Pk−K′HkPkK=PkHkT(HkPkHkT+Rk)−1\begin{align*} {\color{blue}\hat{\mathbf x}_k'} &= {\color{magenta}\hat{\mathbf{x}}_k} + {\color{purple}\mathbf{K}'}({\color{green}\mathbf{z}_k}-{\color{magenta}\mathbf{H}_k\hat{\mathbf{x}}_k}) \\ {\color{blue}\mathbf{P}_k'} &= {\color{orangered}\mathbf{P}_k} - {\color{purple}\mathbf{K}}'{\color{orangered}\mathbf{H}_k\mathbf{P}_k} \\ {\color{purple}\mathbf{K}} &= {\color{orangered}\mathbf{P}_k\mathbf{H}^T_k}({\color{orangered}\mathbf{H}_k\mathbf{P}_k\mathbf{H}^T_k} + {\color{cyan}\mathbf{R}_k})^{-1} \end{align*}

…最终得到更新过程的完整公式。

嗯，这就是卡尔曼滤波过程，x^k′\color{blue}\hat{\mathbf{x}}_k'是卡尔曼最优估计。我们可以将新状态x^k′\color{blue}\hat{\mathbf{x}}_k'及其协方差Pk′\color{blue}\mathbf{P}_k'再代入到新一轮的预测及更新中，且可以循环多次。

参考链接: https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/